////////////////////////////////////////////////////
// anim.cpp version 4.1
// Template code for drawing an articulated figure.
// CS 174A 
////////////////////////////////////////////////////

#ifdef WIN32
#include <windows.h>
#include "GL/glew.h"
#include <GL/gl.h>
#include <GL/glu.h>
#else
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#endif

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <assert.h>

#ifdef WIN32
#include "GL/freeglut.h"
#else
#include <GLUT/glut.h>
#endif

#include "common.h"
#include "Ball.h"
#include "FrameSaver.h"
#include "Timer.h"
#include "tga.h"
#include "engine.h"
#include "optimization.h"

#ifdef __APPLE__
#define glutInitContextVersion(a,b)
#define glutInitContextProfile(a)
#define glewExperimental int glewExperimentalAPPLE
#define glewInit()
#endif

//FrameSaver FrSaver ;
Timer TM ;

BallData *Arcball = NULL ;
int Width = 480 ;
int Height = 480 ;
int Button = -1 ;
float Zoom = 1 ;
int PrevY = 0 ;

int Animate = 0 ;
int Recording = 0 ;
int moving = 0 ;

void resetArcball() ;
void save_image();
void instructions();
void set_colour(float r, float g, float b) ;

const int STRLEN = 100;
typedef char STR[STRLEN];

#define PI 3.1415926535897
#define X 0
#define Y 1
#define Z 2

aerodynamic_parameters aero_coef;
ParamSet *best_paramset;
double timestep = 0.000001;
int currentTP = 0;

ShapeData cubeData;
ShapeData sphereData;
ShapeData ellipsData;
ShapeData coneData;
ShapeData cylData;
ShapeData axisData;
CEngine *engine;

// Matrix stack that can be used to push and pop the modelview matrix.
//class MatrixStack {
//    int    _index;
//    int    _size;
//    mat4*  _matrices;
//
//   public:
//    MatrixStack( int numMatrices = 32 ):_index(0), _size(numMatrices)
//        { _matrices = new mat4[numMatrices]; }
//
//    ~MatrixStack()
//	{ delete[]_matrices; }
//
//    void push( const mat4& m ) {
//        assert( _index + 1 < _size );
//        _matrices[_index++] = m;
//    }
//
//    mat4& pop( void ) {
//        assert( _index - 1 >= 0 );
//        _index--;
//        return _matrices[_index];
//    }
//};
//
//MatrixStack  mvstack;
mat4         model_view;
GLint        uModelView, uProjection, uCameraView;
GLint        uAmbient, uDiffuse, uSpecular, uLightPos, uShininess;
GLint        uTex, uEnableTex;
lighting_struct g_env_lighting;

GLuint program_axis;
GLint uModelView_axis, uProjection_axis, uCameraView_axis;

GLdouble TIME = 0.0 ;
GLdouble last_time_update;

void resetArcball()
{
    Ball_Init(Arcball);
    Ball_Place(Arcball,qOne,0.75);
}


//////////////////////////////////////////////////////
//    PROC: myKey()
//    DOES: this function gets caled for any keypresses
// 
//////////////////////////////////////////////////////

void myKey(unsigned char key, int x, int y)
{
    float time ;
    switch (key) {
        case 'w':
            moving = 1-moving;
            TIME = TM.GetElapsedTime();
            last_time_update = TIME;
            break;
        case 'i':
            moving = 0;            
            engine->init_bird();
            currentTP = 0;
//            TIME = TM.GetElapsedTime();            
//            engine->update(TM.GetElapsedTime()-TIME);
            break;
        case 'q':
        case 27:
            exit(0); 
        case 's':
//            FrSaver.DumpPPM(Width,Height) ;
            break;
        case 'r':
            resetArcball() ;
            glutPostRedisplay() ;
            break ;
        case 'a': // togle animation
            Animate = 1 - Animate ;
            // reset the timer to point to the current time		
            time = TM.GetElapsedTime() ;
            TM.Reset() ;
            // printf("Elapsed time %f\n", time) ;
            break ;
        case '0':
            //reset your object
            break ;
        case 'm':
            if( Recording == 1 )
            {
                printf("Frame recording disabled.\n") ;
                Recording = 0 ;
            }
            else
            {
                printf("Frame recording enabled.\n") ;
                Recording = 1  ;
            }
//            FrSaver.Toggle(Width);
            break ;
        case 'h':
        case '?':
            instructions();
            break;
    }
    glutPostRedisplay() ;

}

/*********************************************************
    PROC: myinit()
    DOES: performs most of the OpenGL intialization
     -- change these with care, if you must.

**********************************************************/

void myinit(void)
{
    engine = new CEngine();
    engine->init();
    engine->build_scene();
    engine->init_bird();
    Arcball = new BallData ;
    Ball_Init(Arcball);
    Ball_Place(Arcball,qOne,0.75);
}
/*********************************************************
**********************************************************
**********************************************************

    PROC: display()
    DOES: this gets called by the event handler to draw
          the scene, so this is where you need to build
          your ROBOT --  
      
        MAKE YOUR CHANGES AND ADDITIONS HERE

    Add other procedures if you like.

**********************************************************
**********************************************************
**********************************************************/
void display(void)
{
    // Clear the screen with the background colour (set in myinit)
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    model_view = mat4(1.0f);
    HMatrix r;
    Ball_Value(Arcball,r);

    mat4 mat_arcball_rot(
        r[0][0], r[0][1], r[0][2], r[0][3],
        r[1][0], r[1][1], r[1][2], r[1][3],
        r[2][0], r[2][1], r[2][2], r[2][3],
        r[3][0], r[3][1], r[3][2], r[3][3]);
    model_view *= mat_arcball_rot;
    engine->display();
    //engine->display_axis();
    glutSwapBuffers();
}

/**********************************************
    PROC: myReshape()
    DOES: handles the window being resized 
    
      -- don't change
**********************************************************/

void myReshape(int w, int h)
{
    Width = w;
    Height = h;

    glViewport(0, 0, w, h);

    mat4 projection = Perspective(50.0f, (float)w/(float)h, 1.0f, 1000.0f);
    glUniformMatrix4fv( uProjection, 1, GL_TRUE, projection );
}

void instructions() 
{
    printf("Press:\n");
    printf("  s to save the image\n");
    printf("  r to restore the original view.\n") ;
    printf("  0 to set it to the zero state.\n") ;
    printf("  a to toggle the animation.\n") ;
    printf("  m to toggle frame dumping.\n") ;
    printf("  q to quit.\n");
}

// start or end interaction
void myMouseCB(int button, int state, int x, int y)
{
    Button = button ;
    if( Button == GLUT_LEFT_BUTTON && state == GLUT_DOWN )
    {
        HVect arcball_coords;
        arcball_coords.x = 2.0*(float)x/(float)Width-1.0;
        arcball_coords.y = -2.0*(float)y/(float)Height+1.0;
        Ball_Mouse(Arcball, arcball_coords) ;
        Ball_Update(Arcball);
        Ball_BeginDrag(Arcball);

    }
    if( Button == GLUT_LEFT_BUTTON && state == GLUT_UP )
    {
        Ball_EndDrag(Arcball);
        Button = -1 ;
    }
    if( Button == GLUT_RIGHT_BUTTON && state == GLUT_DOWN )
    {
        PrevY = y ;
    }


    // Tell the system to redraw the window
    glutPostRedisplay() ;
}

// interaction (mouse motion)
void myMotionCB(int x, int y)
{
    if( Button == GLUT_LEFT_BUTTON )
    {
        HVect arcball_coords;
        arcball_coords.x = 2.0*(float)x/(float)Width - 1.0 ;
        arcball_coords.y = -2.0*(float)y/(float)Height + 1.0 ;
        Ball_Mouse(Arcball,arcball_coords);
        Ball_Update(Arcball);
        glutPostRedisplay() ;
    }
    else if( Button == GLUT_RIGHT_BUTTON )
    {
        if( y - PrevY > 0 )
            Zoom  = Zoom * 1.03 ;
        else 
            Zoom  = Zoom * 0.97 ;
        PrevY = y ;
        glutPostRedisplay() ;
    }
}

void idleCB(void)
{
    if( Animate == 1 )
    {
        // TM.Reset() ; // commenting out this will make the time run from 0
        // leaving 'Time' counts the time interval between successive calls to idleCB
        if( Recording == 0 )
            TIME = TM.GetElapsedTime() ;
        else
            TIME += 0.033 ; // save at 30 frames per second.
        printf("TIME %f\n", TIME) ;
        glutPostRedisplay() ; 
    }
    
    if (moving == 1) {
        engine->update(TM.GetElapsedTime()-last_time_update);
        last_time_update = TM.GetElapsedTime();
        
//        if(currentTP < best_paramset->numTimepoints)
//        {
//            engine->setBirdParamters(1, best_paramset->u[currentTP][0]);
//            engine->setBirdParamters(5, -best_paramset->u[currentTP][0]);
//            engine->update(timestep);
//            currentTP++;
//        }
//        
        glutPostRedisplay() ;
    }
    
}

ParamSet* optimize()
{
    int numIteration = 1000;
    int cntIteration = 0;
    double bestObj;
    ParamSet *bestParam = NULL;
    
    int numController = 1;
    double timestep = 0.001;
    double timespan = 5.0;
    double heat = 0.1;
    double range = 0.1;
    
    bird_parameters init_params;
    init_params.num_params = 11;
    for(int i = 0; i < init_params.num_params; i++)
        engine->setBirdParamters(i, 0);
    
    COptimization* optim = new COptimization();
    double uperBd[1] = {40.0};
    double lwerBd[1] = {-40.0};
    double initVal[1] = {0.0};
    ParamSet *paramSet = optim->initialize(numController, timespan, timestep, uperBd, lwerBd, initVal);
    
    printf("start of optimization, iterations: %d\n", numIteration);
    
    bird_status* statusSet = new bird_status[optim->getNumTimepoints()+1];
    while (cntIteration < numIteration)
    {
        printf("iteration %d\n", cntIteration);
//        printf("    parameters:\n");
//        for(int i = 0; i < paramSet.numController; i++)
//        {
//            for(int j = 0; j < paramSet.numTimepoints; j++)
//            {
//                printf(" %lf", paramSet.u[j][i]);
//            }
//            printf("\n");
//        }
        
        engine->init_bird();

        statusSet[0] = engine->getBirdStatus();
        
        for(int i = 0; i < optim->getNumTimepoints(); i++)
        {
            engine->setBirdParamters(1, paramSet->u[i][0]);
            engine->setBirdParamters(5, -paramSet->u[i][0]);            
            
            engine->update(timestep);
            statusSet[i + 1] = engine->getBirdStatus();
        }
        
        double score = optim->evaluate_objective_function(statusSet, paramSet);
        printf("objective function score: %lf\n", score);
        
        if(cntIteration == 0 || score < bestObj)
        {
            printf("******better!\n");
            bestObj = score;
            if (bestParam != NULL) 
                delete bestParam;
            bestParam = new ParamSet(paramSet);
        }
        else
        {
            printf("======worse!\n");
            delete paramSet;
            paramSet = new ParamSet(bestParam);
        }
        
        optim->update(paramSet, heat, range);
        cntIteration++;
    }
    
    printf("----------optimization complete!\n");
    printf("best score: %lf\n", bestObj);
    
    delete [] statusSet;
    
    return bestParam;
}


/*********************************************************
     PROC: main()
     DOES: calls initialization, then hands over control
           to the event handler, which calls 
           display() whenever the screen needs to be redrawn
**********************************************************/

int main(int argc, char** argv) 
{
    glutInit(&argc, argv);
    // If your code fails to run, uncommenting these lines may help.
    //glutInitContextVersion(3, 2);
    //glutInitContextProfile(GLUT_CORE_PROFILE);
    glutInitDisplayMode (GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
    glutInitWindowPosition (0, 0);
    glutInitWindowSize(Width,Height);
    glutCreateWindow(argv[0]);
    printf("GL version %s\n", glGetString(GL_VERSION));
    glewExperimental = GL_TRUE;
    glewInit();
    
    myinit();

    glutIdleFunc(idleCB) ;
    glutReshapeFunc (myReshape);
    glutKeyboardFunc( myKey );
    glutMouseFunc(myMouseCB) ;
    glutMotionFunc(myMotionCB) ;
    instructions();
    
//    best_paramset = optimize();
//    best_paramset->output("best_param.txt");

    glutDisplayFunc(display);
    glutMainLoop();

    TM.Reset() ;
    return 0;         // never reached
}




